/*
The MIT License (MIT)

Copyright (c) 2013 Mike Dapiran, Brian May, Richard Pospesel, and Bert Wierenga

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#include "hhdQuaternion.h"
#include "hhdMath.h"
#include "hhdMatrix4.h"
#include "hhdVector3.h"

namespace hhd
{
    const Quaternion Quaternion::identity = Quaternion(1.0f, 0.0f, 0.0f, 0.0f);

    Quaternion::Quaternion(void)
    {
        _w = 1.0f;
        _x = 0.0f;
        _y = 0.0f;
        _z = 0.0f;
    }

    Quaternion::Quaternion(float w, float x, float y, float z)
    {
        _w = w;
        _x = x;
        _y = y;
        _z = z;
    }

    Quaternion::Quaternion(float w, const Vector3& v)
    {
        _w = w;
        _x = v.x;
        _y = v.y;
        _z = v.z;
    }

    Quaternion::Quaternion(const Vector3& axis, float angle)
    {
        _w = Math::cos(angle/2.0f);
        _x = Math::sin(angle/2.0f)*axis.x;
        _y = Math::sin(angle/2.0f)*axis.y;
        _z = Math::sin(angle/2.0f)*axis.z;
    }

    Quaternion::Quaternion(const Quaternion& in_source)
    {
        _w = in_source._w;
        _x = in_source._x;
        _y = in_source._y;
        _z = in_source._z;
    }

    Quaternion::Quaternion(float euler_x, float euler_y, float euler_z)
    {
        _w = 1.0f;
        _x = 0.0f;
        _y = 0.0f;
        _z = 0.0f;

        if(euler_x != 0.0f)
        {
            *this = (*this).mul(Quaternion(Vector3::right, euler_x));
        }
        if(euler_y != 0.0f)
        {
            *this = (*this).mul(Quaternion(Vector3::up, euler_y));
        }
        if(euler_z != 0.0f)
        {
            *this = (*this).mul(Quaternion(Vector3::forward, euler_z));
        }
        (*this).normalize();
    }

    Quaternion::~Quaternion(void)
    {
    }

    Quaternion::Quaternion(const Matrix4& matrix)
    {
        setFromBasis(matrix);
    }

    Quaternion Quaternion::getInverse() const
    {
        return Quaternion(_w, -_x, -_y, -_z);
    }

    Vector3 Quaternion::rotate(const Vector3& in_to_transform) const
    {
        //come back to this when we get a fast cross product
        float vector_coeff = 1.0f - _w*_w;
        float vector_part_coeff = 2.0f*(_x*in_to_transform.x + _y*in_to_transform.y + _z*in_to_transform.z);
        float cross_coeff = 2.0f*_w;

        return Vector3(vector_coeff*in_to_transform.x + vector_part_coeff*_x + cross_coeff*(_y*in_to_transform.z - _z*in_to_transform.y),
                       vector_coeff*in_to_transform.y + vector_part_coeff*_y + cross_coeff*(_z*in_to_transform.x - _x*in_to_transform.z),
                       vector_coeff*in_to_transform.z + vector_part_coeff*_z + cross_coeff*(_x*in_to_transform.y - _y*in_to_transform.x));
    }

    Quaternion& Quaternion::operator=(const Quaternion& in_source)
    {
        _w = in_source._w;
        _x = in_source._x;
        _y = in_source._y;
        _z = in_source._z;

        return *this;
    }

    Quaternion Quaternion::operator*(const Quaternion& in_right) const
    {
        Vector3 left = Vector3(_x, _y, _z);
        Vector3 right = Vector3(in_right._x, in_right._y, in_right._z);

        float w = _w*in_right._w - Vector3::dot(left, right);
        Vector3 v = _w*right + in_right._w*left + Vector3::cross(right, left);

        Quaternion q(w,v);
        return q.normalize();
    }

    Quaternion Quaternion::operator*(float in_right) const
    {
        return Quaternion(_w*in_right, _x*in_right, _y*in_right, _z*in_right);
    }

    Quaternion operator*(float in_left, const Quaternion& in_right)
    {
        return Quaternion(in_right._w*in_left, in_right._x*in_left, in_right._y*in_left, in_right._z*in_left);
    }

    Quaternion Quaternion::operator+(const Quaternion& in_right) const
    {
        return Quaternion(_w+in_right._w, _x+in_right._x, _y+in_right._y, _z+in_right._z);
    }

    Quaternion Quaternion::operator/(float right) const
    {
        if( Math::abs(right) < 0.0001f)
        {
            error("division by zero");
        }

        return Quaternion(_w/right, _x/right, _y/right, _z/right);
    }

    Quaternion Quaternion::mul(const Quaternion& in_right) const
    {
        Vector3 left = Vector3(_x, _y, _z);
        Vector3 right = Vector3(in_right._x, in_right._y, in_right._z);

        float w = _w*in_right._w - Vector3::dot(left, right);
        Vector3 v = _w*right + in_right._w*left + Vector3::cross(right, left);

        return Quaternion(w,v);
    }

    Quaternion& Quaternion::toIdentifty()
    {
        _w = 1.0f;
        _x = 0.0f;
        _y = 0.0f;
        _z = 0.0f;

        return *this;
    }

    Quaternion& Quaternion::normalize()
    {
        float magnitude = Math::sqrt( _w*_w + _x*_x + _y*_y + _z*_z );
        if( magnitude < 0.001f)
        {
            error("zero length quaternion");
        }

        _w /= magnitude;
        _x /= magnitude;
        _y /= magnitude;
        _z /= magnitude;
        return *this;
    }

    float Quaternion::dot(const Quaternion& left, const Quaternion& right)
    {
        return (left._w*right._w) + (left._x*right._x) + (left._y*right._y) + (left._z*right._z);
    }

    float Quaternion::angleBetween(const Quaternion& left, const Quaternion& right)
    {
        return Math::acos( Quaternion::dot( left, right ) );
    }

    Quaternion Quaternion::slerp(const Quaternion& start, const Quaternion& end, float t)
    {
        if( t < 0.0f)
        {
            t = 0.0f;
        }
        else if( t > 1.0f )
        {
            t = 1.0f;
        }

        float theta = angleBetween( start, end );
        float sineTheta = Math::sin(theta);

        if(Math::abs(sineTheta) < 0.001f)
        {
            return end;
        }

        return ( (Math::sin((1-t)*theta)*start)  +  (Math::sin(t*theta)*end) ) /sineTheta;
    }

    Quaternion Quaternion::intermediateSlerp( const Quaternion& low_end, const Quaternion& intermediate, const Quaternion& high_end, float t)
    {
        if(t > 1.0f)
        {
            t = 1.0f;
        }
        else if( t < 0.0f)
        {
            t = 0.0f;
        }

        if( t < 0.5f )
        {
            return Quaternion::slerp(intermediate, low_end, 1.0f-(t*2.0f) );
        }
        else
        {
            return Quaternion::slerp(intermediate, high_end, (t-0.5f)*2.0f);
        }
    }

    Matrix4 Quaternion::toMatrix4() const
    {
        return Matrix4(*this);
    }

    Vector3 Quaternion::getForward() const
    {

        //s = 2.0 since quaternions are always kept normalized
        float s = 2.0f;

        float xs = s*_x;
        float ys = s*_y;
        float zs = s*_z;

        float wx = _w*xs;
        float wy = _w*ys;

        float xx = _x*xs;
        float xz = _x*zs;
        float yy = _y*ys;
        float yz = _y*zs;

        return Vector3(xz+wy, yz-wx, 1.0f - (xx+yy));
    }

    Vector3 Quaternion::getUp() const
    {
        //s = 2.0 since quaternions are always kept normalized
        float s = 2.0f;

        float xs = s*_x;
        float ys = s*_y;
        float zs = s*_z;

        float wx = _w*xs;
        float wz = _w*zs;

        float xx = _x*xs;
        float xy = _x*ys;
        float yz = _y*zs;
        float zz = _z*zs;

        return Vector3(xy-wz, 1.0f - (xx+zz), yz+wx);
    }

    Vector3 Quaternion::getRight() const
    {
        //s = 2.0 since quaternions are always kept normalized
        float s = 2.0f;

        float ys = s*_y;
        float zs = s*_z;

        float wy = _w*ys;
        float wz = _w*zs;

        float xy = _x*ys;
        float xz = _x*zs;
        float yy = _y*ys;
        float zz = _z*zs;

        return Vector3(1.0f - (yy+zz), xy+wz, xz-wy);
    }

    Quaternion& Quaternion::setFromBasis(const Matrix4& matrix)
    {
        Matrix4 m;

        m.setRight(matrix.getRight().normalize());
        m.setUp(matrix.getUp().normalize());
        m.setForward(matrix.getForward().normalize());

        float trace = m(0,0) + m(1,1) + m(2,2);
        if(trace > 0)
        {
            float s = (1.0f / Math::sqrt(trace + 1.0f)) * 0.5f;
            _w = s * (trace + 1.0f);
            _x = (m(2,1) - m(1,2)) * s;
            _y = (m(0,2) - m(2,0)) * s;
            _z = (m(1,0) - m(0,1)) * s;
        }
        else
        {
            if(m(0,0) > m(1,1) && m(0,0) > m(2,2))
            {
                float s = 2.0f * Math::sqrt(1.0f + m(0,0) - m(1,1) - m(2,2));
                _w = (m(2,1) - m(1,2)) / s;
                _x = 0.25f * s;
                _y = (m(0,1) + m(1,0)) / s;
                _z = (m(0,2) + m(2,0)) / s;
            }
            else if(m(1,1) > m(2,2))
            {
                float s = 2.0f * Math::sqrt(1.0f + m(1,1) - m(0,0) - m(2,2));
                _w = (m(0,2) - m(2,0)) / s;
                _x = (m(0,1) + m(1,0)) / s;
                _y = 0.25f * s;
                _z = (m(1,2) + m(2,1)) / s;
            }
            else
            {
                float s = 2.0f * Math::sqrt(1.0f + m(2,2) - m(0,0) - m(1,1));
                _w = (m(1,0) - m(0,1)) / s;
                _x = (m(0,2) + m(2,0)) / s;
                _y = (m(1,2) + m(2,1)) / s;
                _z = 0.25f * s;
            }
        }
        return *this;
    }

    Quaternion& Quaternion::setFromBasis(const Vector3& forward, const Vector3& up)
    {
        //get orthonormal basis
        Vector3 f(forward);

        Vector3 u(up);

        Vector3 r(Vector3::cross(u,f));

        u = Vector3::cross(f,r);

        Matrix4 m;

        m.setRight(r.normalize());
        m.setUp(u.normalize());
        m.setForward(f.normalize());

        float trace = m(0,0) + m(1,1) + m(2,2);
        if(trace > 0)
        {
            float s = (1.0f / Math::sqrt(trace + 1.0f)) * 0.5f;
            _w = s * (trace + 1.0f);
            _x = (m(2,1) - m(1,2)) * s;
            _y = (m(0,2) - m(2,0)) * s;
            _z = (m(1,0) - m(0,1)) * s;
        }
        else
        {
            if(m(0,0) > m(1,1) && m(0,0) > m(2,2))
            {
                float s = 2.0f * Math::sqrt(1.0f + m(0,0) - m(1,1) - m(2,2));
                _w = (m(2,1) - m(1,2)) / s;
                _x = 0.25f * s;
                _y = (m(0,1) + m(1,0)) / s;
                _z = (m(0,2) + m(2,0)) / s;
            }
            else if(m(1,1) > m(2,2))
            {
                float s = 2.0f * Math::sqrt(1.0f + m(1,1) - m(0,0) - m(2,2));
                _w = (m(0,2) - m(2,0)) / s;
                _x = (m(0,1) + m(1,0)) / s;
                _y = 0.25f * s;
                _z = (m(1,2) + m(2,1)) / s;
            }
            else
            {
                float s = 2.0f * Math::sqrt(1.0f + m(2,2) - m(0,0) - m(1,1));
                _w = (m(1,0) - m(0,1)) / s;
                _x = (m(0,2) + m(2,0)) / s;
                _y = (m(1,2) + m(2,1)) / s;
                _z = 0.25f * s;
            }
        }
        return *this;
    }

    Quaternion& Quaternion::setFromAxisAngle(const Vector3& axis, float angle)
    {
        _w = Math::cos(angle/2.0f);
        _x = Math::sin(angle/2.0f)*axis.x;
        _y = Math::sin(angle/2.0f)*axis.y;
        _z = Math::sin(angle/2.0f)*axis.z;
        return *this;
    }

    PrintStream& operator<<(PrintStream& ps, const Quaternion& q)
    {
        ps << q.w() << " " << q.x() << " " << q.y() << " " << q.z();
        return ps;
    }
}